#if defined __linux__ || defined __APPLE__
	#include <pthread.h>
#elif _WIN32 //defined for both 32 and 64 bit windows platform
	// undef needed for LCC compiler
	#undef EXTERN_C
	#include <windows.h>
	#include <process.h>
#endif

#include <map>
#include <mex.h>
#include "mexutil.h"

typedef std::pair<float,unsigned int> PAIR;
typedef std::multimap<float,unsigned int> MULTIMAP;
typedef MULTIMAP::iterator ITER;
unsigned int threadN , nt , nr , Nv , pp, KNN;
float *    ptsx , *    ptsy , *    ptsz , *    ptsx_norm , *    ptsy_norm , *    ptsz_norm ;
float * pp_ptsx , * pp_ptsy , * pp_ptsz , * pp_ptsx_norm , * pp_ptsy_norm , * pp_ptsz_norm ;
char  * filter;
float pol_r , eps;

typedef struct Data
{
	unsigned int ThreadID;
} DataType;

mwSize dims[2];

unsigned int ** PPSphere2PolarSphere;
unsigned int * PP2PolarMapIdx;
float * PP2PolarMapDist;

#if defined __linux__ || defined __APPLE__
#define max(A,B) (A>B?A:B)
#define min(A,B) (A<B?A:B)
#endif

unsigned int calculateMap(unsigned int pp_idx)
{
	unsigned int knn , idx_ang , idx_plr , r_rat , idx_max , idx_min;
	unsigned int idx_plr_sphere , idx_plr_origin ;
	float dx , dy , dz , d , pp_x , pp_y , pp_z , pp_r;
	MULTIMAP Multimap;
	ITER it;
	pp_x = pp_ptsx[pp_idx];
	pp_y = pp_ptsy[pp_idx];
	pp_z = pp_ptsz[pp_idx];
	pp_r = sqrt(pp_x*pp_x+pp_y*pp_y+pp_z*pp_z);
	r_rat=(unsigned int)floor((1.0-pp_r/pol_r)*(nr-1));
	//idx_ang is the index on the outer facet 
	idx_ang = (pp_idx > ((Nv+1)*(Nv+1)*(3*Nv+1)/2) ? 
			   pp_idx % ((Nv+1)*(Nv+1)) + (Nv+1)*(Nv+1) : 
			   pp_idx % ((Nv+1)*(Nv+1)) );
	if (pp_r <= pol_r)
	{
		for (knn = 0; knn < KNN ; ++knn)
		{
			//idx_plr_sphere the index of the polar point on the outer sphere
			idx_plr_sphere = PPSphere2PolarSphere[idx_ang][knn];
			//idx_plr_origin the index of the polar point at the origin
			//which is on the same ray as idx_plr_sphere
			idx_plr_origin = idx_plr_sphere - (nr-1);
			//idx_plr the approximated location of pp_idx neighbors
			//no need to check more than +/-(2) points
			//and anyway they must be on the same ray
			idx_plr = idx_plr_sphere - r_rat;
			idx_min = max(idx_plr-2,idx_plr_origin);
			idx_max = min(idx_plr+2,idx_plr_sphere);
			for (idx_plr = idx_min; idx_plr<=idx_max; ++idx_plr)
			{
				dx=pp_x-ptsx[idx_plr];
				dy=pp_y-ptsy[idx_plr];
				dz=pp_z-ptsz[idx_plr];
				d=dx*dx+dy*dy+dz*dz;
				if ( Multimap.size() < KNN )
				{
					//at first insert KNN points to Multimap
					Multimap.insert(PAIR(d,idx_plr));
				}
				else if ((*(--(it=Multimap.end()))).first > d)
				{
					//a closer point than what we have so far was found
					//erase the furthest point and insert the new one
					Multimap.erase(it);
					Multimap.insert(PAIR(d,idx_plr));
				}
			}
		}
	}
	else
	{
		//pp point is outside the sphere
		for (knn = 0; knn < KNN ; ++knn)
		{
			//dummy point, negative distance is a flag for matlab to ignore this value
			Multimap.insert(PAIR(-1.0,0));
		}
	}
	knn = 0;
	for (it = Multimap.begin() ; it != Multimap.end() ; ++it)
	{
		PP2PolarMapIdx [dims[0]*pp_idx + knn]=(*it).second;
		PP2PolarMapDist[dims[0]*pp_idx + knn]=(*it).first;
		++knn;
	}
	return knn;
}

#if defined __linux__ || defined __APPLE__
	void * calculatePP2PolarMap(void * Args_in)
#elif _WIN32 //defined for both 32 and 64 bit windows platform
    unsigned __stdcall calculatePP2PolarMap(void * Args_in)
#endif
{
	DataType *Args = static_cast<DataType*>(Args_in);
    unsigned int ThreadID = Args->ThreadID , ret;
    //this function creates a map from each pp index to its 8 knn
    //pp origin is mapped to polar origin
    //outer pp facets are mapped to the outer sphere of the polar grid
	//reset of the points in the middle are mapped according to their relative location
	//with respect to the radial variable
	for (unsigned int pp_idx = ThreadID ; pp_idx < (Nv+1)*(Nv+1)*(3*Nv+1) ; pp_idx+=threadN)
	{
		if ( KNN != ( ret = calculateMap(pp_idx) ))
		{
			mexPrintf("Error, calculateMap inserted %d points for pp %d pp_idx %d\n",
				ret,pp,pp_idx);
		}
	}
	
	#ifdef _WIN32 //defined for both 32 and 64 bit windows platform
        // explicit end thread, helps to ensure proper recovery of resources allocated for the thread
        _endthreadex( 0 );
    #endif
    return 0;
}

unsigned int searchKnn(unsigned int pp_idx)
{
	float dx , dy , dz , d ;
	float x = pp_ptsx_norm[ pp_idx ];
	float y = pp_ptsy_norm[ pp_idx ];
	float z = pp_ptsz_norm[ pp_idx ];
	//This data structure stores points indexes with distance
	//The ordering is according to the distance
	MULTIMAP Multimap;
	ITER it;
	char flag;
	for (unsigned int polar_idx = 0 ; polar_idx < nt ; ++polar_idx)
	{
		//check with the filter if polar_idx is worth to be checked
		flag=filter[polar_idx];
		if (flag == 0)
			continue;
		if (flag == 1)
		{
			if ((pp == 1 && x<0) || (pp == 2 && y<0) || (pp == 3 && z<0))
				continue;
		}
		else if (flag == -1)
		{
			if ((pp == 1 && x>0) || (pp == 2 && y>0) || (pp == 3 && z>0))
				continue;
		}

		d=0;
		if (1 == pp)
		{
			dy=ptsy_norm[polar_idx]-y;
			d+=dy*dy;
			if (d > eps) continue;
			dz=ptsz_norm[polar_idx]-z;
			d+=dz*dz;
			if (d > eps) continue;
			dx=ptsx_norm[polar_idx]-x;
			d+=dx*dx;
			if (d > eps) continue;
		}
		else if (2 == pp)
		{
			dx=ptsx_norm[polar_idx]-x;
			d+=dx*dx;
			if (d > eps) continue;
			dz=ptsz_norm[polar_idx]-z;
			d+=dz*dz;
			if (d > eps) continue;
			dy=ptsy_norm[polar_idx]-y;
			d+=dy*dy;
			if (d > eps) continue;
		}
		else if (3 == pp)
		{
			dx=ptsx_norm[polar_idx]-x;
			d+=dx*dx;
			if (d > eps) continue;
			dy=ptsy_norm[polar_idx]-y;
			d+=dy*dy;
			if (d > eps) continue;
			dz=ptsz_norm[polar_idx]-z;
			d+=dz*dz;
			if (d > eps) continue;
		}


		if ( Multimap.size() < KNN )
		{
			//at first insert KNN points to Multimap
			Multimap.insert(PAIR(d,(polar_idx+1)*nr-1));
		}
		else if ((*(--(it=Multimap.end()))).first > d)
		{
			//a closer point than what we have so far was found
			//erase the furthest point and insert the new one
			Multimap.erase(it);
			Multimap.insert(PAIR(d,(polar_idx+1)*nr-1));
		}
	}
	//all nt distances between pp point pp_idx and all polar points were compared
	//write KNN points to the output variable
	unsigned int knn=0;
	for (it = Multimap.begin() ; it != Multimap.end() ; ++it)
	{
		PPSphere2PolarSphere[pp_idx][knn++]=(*it).second;
	}
	return knn;
}

#if defined __linux__ || defined __APPLE__
	void * makeKnnSphere(void * Args_in)
#elif _WIN32 //defined for both 32 and 64 bit windows platform
    unsigned __stdcall makeKnnSphere(void * Args_in)
#endif
{
	DataType *Args = static_cast<DataType*>(Args_in);
    unsigned int ThreadID = Args->ThreadID, ret;
	//for each pp point on the outer facets measure its distance from all polar points on the outer sphere
	//and return the KNN closest points, this is a quadratic algorithm
	for (unsigned int pp_idx = ThreadID ; pp_idx < 2*(Nv+1)*(Nv+1) ; pp_idx+=threadN)
	{
		if ( KNN != ( ret = searchKnn(pp_idx) ))
		{
			mexPrintf("Error, searchKnn inserted %d points for pp %d pp_idx %d\n",
				ret,pp,pp_idx);
		}
	}
	#ifdef _WIN32 //defined for both 32 and 64 bit windows platform
        // explicit end thread, helps to ensure proper recovery of resources allocated for the thread
        _endthreadex( 0 );
    #endif
    return 0;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	unsigned int i;
	if (nrhs!=19) {
		mexPrintf("Error, 19 parameters are required: %s%s\n",
			"threadN,nt,nr,Nv,pp,KNN,ptsx,ptsy,ptsz,pp_ptsx,pp_ptsy,pp_ptsz,filter,",
			"ptsx_norm,ptsy_norm,ptsz_norm,pp_ptsx_norm,pp_ptsy_norm,pp_ptsz_norm");
	}
	// Get Input parameters
	GetUInt(prhs[0],&threadN);
	GetUInt(prhs[1],&nt);
	GetUInt(prhs[2],&nr);
	GetUInt(prhs[3],&Nv);
	GetUInt(prhs[4],&pp);
	GetUInt(prhs[5],&KNN);
	ptsx         =(float *)mxGetPr(prhs[6]);
	ptsy         =(float *)mxGetPr(prhs[7]);
	ptsz         =(float *)mxGetPr(prhs[8]);
	pp_ptsx      =(float *)mxGetPr(prhs[9]);
	pp_ptsy      =(float *)mxGetPr(prhs[10]);
	pp_ptsz      =(float *)mxGetPr(prhs[11]);
	filter       =(char  *)mxGetPr(prhs[12]);
	ptsx_norm    =(float *)mxGetPr(prhs[13]);
	ptsy_norm    =(float *)mxGetPr(prhs[14]);
	ptsz_norm    =(float *)mxGetPr(prhs[15]);
	pp_ptsx_norm =(float *)mxGetPr(prhs[16]);
	pp_ptsy_norm =(float *)mxGetPr(prhs[17]);
	pp_ptsz_norm =(float *)mxGetPr(prhs[18]);

	pol_r=sqrt(ptsx[nr-1]*ptsx[nr-1]+ptsy[nr-1]*ptsy[nr-1]+ptsz[nr-1]*ptsz[nr-1]);

	//there are Nv^2 squares on a pp facet, after mapping it to the unit sphere
	//it is exactly 1/6 of it. If the polar points are uniformly distributed
	//and if we assume there are at least alpha*nt points on the mapped facet
	//then we conclude there are alpha*nt/(Nv^2) polar points for every pp point
	//a great circle on the unit sphere is 2*PI long so the pp facet side is not more
	//than PI/2, so the distance between 2 pp points is less than PI/(2Nv)
	//alpha for a rough estimation should be less than 1/6,
	//the ratio of the surface of a circle on the sphere inscribed in the 
	//pp facet and the entire sphere is (2-sqrt(2))/4 < 1/6
	//KNN should be proportional to eps
	//eps is squared since the output distances are squared distances
	//this is a very rough estimation it is possible to tighten it a lot

	eps = (4*atan(1.0)/(2*Nv))*((2-sqrt(2.0))/4)*(nt/(Nv*Nv));
	eps = eps*eps*KNN;

	PPSphere2PolarSphere = new unsigned int * [2*(Nv+1)*(Nv+1)];
	for (i = 0 ; i < 2*(Nv+1)*(Nv+1) ; ++i)
		PPSphere2PolarSphere[i] = new unsigned int [KNN];

	// Allocate output variable
	dims[0]=KNN; dims[1]=(Nv+1)*(Nv+1)*(3*Nv+1);

	plhs[0] = mxCreateNumericArray(2 , dims , mxUINT32_CLASS , mxREAL);
	plhs[1] = mxCreateNumericArray(2 , dims , mxSINGLE_CLASS , mxREAL);

	PP2PolarMapIdx  = (unsigned int *) mxGetPr(plhs[0]);
	PP2PolarMapDist = (float        *) mxGetPr(plhs[1]);
	
	// store all needed function variables
    DataType * ThreadArgs = (DataType *) mxCalloc(threadN , sizeof( DataType ));

    #if defined __linux__ || defined __APPLE__
		pthread_t * ThreadList = (pthread_t *) mxMalloc(threadN * sizeof( pthread_t ));
    #elif _WIN32 //defined for both 32 and 64 bit windows platform
		// Handles to the worker threads
		// Reserve room for handles of threads in ThreadList
		HANDLE * ThreadList = (HANDLE *) mxMalloc(threadN * sizeof( HANDLE ));
	#endif

	for (i = 0 ; i < threadN ; i++) {
		// initilaize thread arguments
		ThreadArgs[i].ThreadID = i ;
		#if defined __linux__ || defined __APPLE__
			pthread_create(&(ThreadList[i]),NULL,&makeKnnSphere,(void *)&(ThreadArgs[i]));
		#elif _WIN32 //defined for both 32 and 64 bit windows platform
			ThreadList[i] = (HANDLE)_beginthreadex(NULL,0,&makeKnnSphere,(void *)&(ThreadArgs[i]),0,NULL);
		#endif
	}
    #if defined __linux__ || defined __APPLE__
		for (i = 0 ; i < threadN ; i++) {
			pthread_join( ThreadList[i], NULL);
		}
    #elif _WIN32 //defined for both 32 and 64 bit windows platform
        // Normally, the next code line is used to wait for threads to finish
        WaitForMultipleObjects(threadN,ThreadList,true,INFINITE);
    #endif

	for (i = 0 ; i < threadN ; i++) {
		// create separate thread and start the KnnMakeHemisphere function.
		#if defined __linux__ || defined __APPLE__
			pthread_create(&(ThreadList[i]),NULL,&calculatePP2PolarMap,(void *)&(ThreadArgs[i]));
		#elif _WIN32 //defined for both 32 and 64 bit windows platform
			ThreadList[i] = (HANDLE)_beginthreadex(NULL,0,&calculatePP2PolarMap,(void *)&(ThreadArgs[i]),0,NULL);
		#endif
	}
	#if defined __linux__ || defined __APPLE__
		for (i = 0 ; i < threadN ; i++) {
			pthread_join( ThreadList[i], NULL);
		}
	#elif _WIN32 //defined for both 32 and 64 bit windows platform
		// Normally, the next code line is used to wait for threads to finish
		WaitForMultipleObjects(threadN,ThreadList,true,INFINITE);
	#endif
	
	for (i = 0 ; i < 2*(Nv+1)*(Nv+1) ; ++i)
		delete [] PPSphere2PolarSphere[i];
	delete [] PPSphere2PolarSphere;
}
